#include "CollisionHul.h"
CollisionHul::CollisionHul( )
{
	_center = new Vector3();
	_radius = new float;
	
}

CollisionHul::~CollisionHul( )
{
	delete _center;
	delete _radius;
}

void CollisionHul::ComputeHul()
{
	//cout <<"CollisionHul::ComputeHul()"<<endl;


	// berchnet den mittelpunkt und den radius fuer die Huelle
	///////////////////////////////////////////////////////////
	
	// dreieckliste
	list<TrianglePlane*>* triList = getTrianglePlaneList();
	//cout <<"COLL_TRI_PLANE_LIST_SIZE: "<<triList->size()<<endl;
	// iterator durch die dreiecke
	list<TrianglePlane*>::iterator it = triList->begin();

	// durchlaufe der dreiecke
	Vector3 min( 0.0,0.0,0.0 );
	Vector3 max( 0.0,0.0,0.0 );

	while( it != triList->end() )
	{
		min.setX( GeoMath::Min( min.getX(),GeoMath::Min( (*it)->getP3().getX(), GeoMath::Min( (*it)->getP1().getX(), (*it)->getP2().getX() ) ) ));
		min.setY( GeoMath::Min( min.getY(), GeoMath::Min( (*it)->getP3().getY(), GeoMath::Min( (*it)->getP1().getY(), (*it)->getP2().getY() ) ) ));
		min.setZ( GeoMath::Min( min.getZ(), GeoMath::Min( (*it)->getP3().getZ(), GeoMath::Min( (*it)->getP1().getZ(), (*it)->getP2().getZ() ) ) ));

		max.setX( GeoMath::Max( max.getX(), GeoMath::Max( (*it)->getP3().getX(), GeoMath::Max( (*it)->getP1().getX(), (*it)->getP2().getX() ) ) ));
		max.setY( GeoMath::Max( max.getY(), GeoMath::Max( (*it)->getP3().getY(), GeoMath::Max( (*it)->getP1().getY(), (*it)->getP2().getY() ) ) ));
		max.setZ( GeoMath::Max( max.getZ(), GeoMath::Max( (*it)->getP3().getZ(), GeoMath::Max( (*it)->getP1().getZ(), (*it)->getP2().getZ() ) ) ));

		it++;
	}
	//cout <<"MIN: "<<min<<endl;
	//cout <<"MAX: "<<max<<endl;
	Vector3 half = ( max - min ) / 2.0;
	*_center = ( min + half );
	*_radius = half.getAbsolute();
	//cout <<"RADIUS: "<<*_center<<endl;
	//cout <<"RADIUS: "<<*_radius<<endl;
}

void CollisionHul::setTransformation( const Matrix3& mat )
{
	//cout <<"CollisionHul::setTransformation( const Matrix3& mat )"<<endl;

	Vector3 radiusRay(1.0,1.0,1.0);
	radiusRay.setLength( *_radius );
	radiusRay = mat.MultNormalNotNormalize( radiusRay );
	//radiusRay = (*_center) + radiusRay;
	//radiusRay = mat * radiusRay;

	//*_center = mat * (*_center);

	//radiusRay = radiusRay - (*_center);
	//(*_radius) = radiusRay.getAbsolute();
	//cout <<"RADIUS: "<<*_radius<<endl;
	ComputeHul ();
}

void CollisionHul::addChildHul( int id, CollisionHul* childHul )
{
	map<int,CollisionHul*>::iterator it = _childHuls.find( id );
	if( it == _childHuls.end() ){
		_childHuls.insert( pair<int,CollisionHul*>(id, childHul));
		childHul->ComputeHul();
		return;
	}
	throw "ID allready exists in _childHuls in CollisionHul::addChildHul( int id, CollisionHul* childHul )";
}

void CollisionHul::removeChildHul( int id )
{
	map<int,CollisionHul*>::iterator it = _childHuls.find( id );
	if( it == _childHuls.end() )
		throw "ID not exists in _childHuls in CollisionHul::removeChildHul( int id  )";
	_childHuls.erase( it );
}


map<int,CollisionHul*>* CollisionHul::getChildHuls( )
{
	return &_childHuls;
};

Vector3 CollisionHul::getCenter( )
{
	return *_center;
};

float CollisionHul::getRadius( )
{
	return *_radius;
};




